{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true,
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "## Алгоритм A* для планирования пути по 2D карте"
   ]
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Импорты"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "from robobase.routeA import AStarGrid\n",
    "from src.support import startAndEntToMap, routeToMap"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Входные параметры"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "outputs": [],
   "source": [
    "input_map = [[0, 0, 0, 1, 1, 1, 0, 0],\n",
    "             [0, 0, 0, 0, 0, 1, 0, 0],\n",
    "             [0, 0, 0, 1, 0, 1, 0, 0],\n",
    "             [0, 0, 0, 1, 0, 1, 0, 0],\n",
    "             [0, 0, 0, 1, 0, 1, 0, 0],\n",
    "             [0, 0, 0, 1, 0, 0, 0, 0],\n",
    "             [0, 0, 0, 1, 0, 1, 1, 0],\n",
    "             [0, 0, 0, 1, 0, 1, 0, 0]]\n",
    "\n",
    "start_coords, dest_coords = [7, 1], [7, 6]"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Построение карты с начальной и конечной координатой"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 288x288 with 1 Axes>",
      "image/png": "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\n"
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "map_with_start_end = startAndEntToMap(input_map, start_coords, dest_coords)\n",
    "plt.matshow(map_with_start_end)\n",
    "plt.show()"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Расчет маршрута"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "outputs": [],
   "source": [
    "route = AStarGrid(input_map, start_coords, dest_coords)"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  },
  {
   "cell_type": "markdown",
   "source": [
    "#### Построение карты с начальной, конечной координатой,  маршрутом"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 288x288 with 1 Axes>",
      "image/png": "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\n"
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "map_with_all = routeToMap(map_with_start_end, route)\n",
    "plt.matshow(map_with_start_end)\n",
    "plt.show()\n",
    "\n",
    "\n"
   ],
   "metadata": {
    "collapsed": false,
    "pycharm": {
     "name": "#%%\n"
    }
   }
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}